#include "./PidCotroler.h"

PIController::PIController(float kp, float ki, float integralLimit, float outputLimit)
    : kp(kp), ki(ki), integral(0.0f), integralLimit(integralLimit), outputLimit(outputLimit) {}

float PIController::compute(float setpoint, float actualValue) {
    float error = setpoint - actualValue;
    integral += error;

    // 积分限幅
    if (integral > integralLimit) integral = integralLimit;
    if (integral < -integralLimit) integral = -integralLimit;

    float output = kp * error + ki * integral;

    // 输出限幅
    if (output > outputLimit) output = outputLimit;
    if (output < -outputLimit) output = -outputLimit;

    return output;
}

void PIController::reset() {
    integral = 0.0f;
}